#include <iostream>
#include <core/controller.h>
#include <path_planning/path_planner.h>
#include <reactive_behavior/reactive_behavior.h>
#include <libplayerinterface/player.h>
#include <boost/thread/thread.hpp>

using std::cout;
using std::cin;
using std::endl;

Controller::Controller(PathPlanner *path_planner,
        ReactiveBehavior *reactive_behavior,
        SyncedCurrentPose *synced_current_pose,
        SyncedCurrentMission *synced_current_mission) :
        path_planner_(path_planner),
                reactive_behavior_(reactive_behavior),
                synced_current_pose_(synced_current_pose),
                synced_current_mission_(synced_current_mission) {}

void Controller::operator()() {
  while (true) {
    double goal_yaw;
    player_pose2d goal;
    player_point_2d goal_position;
    player_point_2d initial_position;

    // Get mission.
    //cout << "Please input mission (x, y, a): ";
    //cin >> goal.px >> goal.py >> goal.pa;
    do {
    	boost::this_thread::sleep(boost::posix_time::milliseconds(300));
    	goal = synced_current_mission_->current_mission();
    } while (synced_current_mission_->accomplished());

    cout << "ABBA: " << goal.px << " " << goal.py << " " << goal.pa << endl;

    // Set mission.
    goal_position.px = goal.px;
    goal_position.py = goal.py;
    goal_yaw = goal.pa;

    // Get current position.
    initial_position = synced_current_pose_->current_position();

    // Request plan and follow it using reactive behavior.
    WaypointList *waypoint_list = path_planner_->RequestWaypoints(initial_position, goal_position);
    player_point_2d *waypoint;
    player_point_2d *prev_waypoint = NULL;
    if (waypoint_list == NULL) goto end_of_mission;
    while ((waypoint = waypoint_list->Next()) != NULL) {
      bool succeded = reactive_behavior_->MoveToGoalAvoidingObstacles(*waypoint);
      if (!succeded) {
        if (prev_waypoint == NULL) {
          // Can't reach route start. Insist with reactive only.
          while (!(succeded = reactive_behavior_->MoveToGoalAvoidingObstacles(*prev_waypoint)));
        }
        WaypointList *tmp_waypoint_list = path_planner_->RequestAlternativeRoute(
                synced_current_pose_->current_position(),
                goal_position,
                *prev_waypoint,
                *waypoint);
        if (tmp_waypoint_list == NULL) goto end_of_mission;
        player_point_2d *alternative = tmp_waypoint_list->Head();
        // Check if the robot got caught up in the unreachable
        // waypoint's influence zone.
        if (alternative->px == waypoint->px &&
                alternative->py == waypoint->py) {
          // Move back using reactive only and try again...
          while (!(succeded = reactive_behavior_->MoveToGoalAvoidingObstacles(*prev_waypoint)));
          delete tmp_waypoint_list;
          tmp_waypoint_list = path_planner_->RequestAlternativeRoute(
                  synced_current_pose_->current_position(),
                  goal_position,
                  *prev_waypoint,
                  *waypoint);
          if (tmp_waypoint_list == NULL) goto end_of_mission;
        }
        // Now release the old list and point to the new list.
        delete waypoint_list;
        waypoint_list = tmp_waypoint_list;
        waypoint = NULL;
      }
      prev_waypoint = waypoint;
    }
end_of_mission:
    // Move from last waypoint to goal and rotate to *goal yaw* orientation.
    //reactive_behavior_->MoveToGoalAvoidingObstacles(goal_position);
    //reactive_behavior_->RotateTo(goal_yaw);
    synced_current_mission_->set_accomplished(true);
  }
}
